#include <math.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <cstring>
// #include <cstdio>
// #include <iostream>
// #include <iomanip>
// #include <cstdlib>

#include <sys/types.h>
#include <sys/stat.h> 

// #include <mkl_lapack.h>
// #include <mkl_cblas.h>

//#include <mkl_lapack.h>
#include <gsl_cblas.h>

// #include "fe_datatypes.h"

#include "fe_vecmat.h"

// #include "fe_3d.h"

using namespace std;

#ifndef _FE_VECMAT_CXX_
#define _FE_VECMAT_CXX_

/* ****************************************************************** */

void FreeDblArray3D( double *** &A, const int n, const int m )	{
	int i,j;
	if( A != 0 ) {
		for(i=0;i<n;i++)
		{
			for(j=0;j<m;j++)
			{
				delete [] A[i][j];
				A[i][j] = 0;
			}
			delete [] A[i];
			A[i] = 0;
		}
		delete [] A;
		A = 0;
	}
}

/* ****************************************************************** */

void FreeDblArray1D( double * &x)
{
	if( x != 0 ) delete [] x;
	x = 0;
}

void NewDblArray1D( double * &x, const int n )	{
	FreeDblArray1D( x );
	x = new double [n];
}

double * NewDblArray1D( const int n )	{
	double * x = 0;
	NewDblArray1D(x,n);
	return x;
}

/* ****************************************************************** */

void FreeDblArray2D( double ** &A, const int n )	{
	if( A != 0 ) {
		int i;
		for(i=0;i<n;i++)
			{	delete [] A[i]; A[i] = 0;	}
		
		delete [] A;	A = 0;
	}
}

double ** NewDblArray2D( const int m, const int n )
{
	double ** A = new double * [m];
	int i;
	for(i=0;i<m;i++) A[i] = new double [n];
	return A;
}

/* ****************************************************************** */

Vector::Vector()
{
	X = 0; n = 0;
}

Vector::Vector(const Vector & y)
{
	n = y.n;
	X = NewDblArray1D(n);
	memcpy(X,y.X,sizeof(double)*n);
}

Vector::Vector(const int N, const bool del)
{
	n=N;
	X = NewDblArray1D(n);
	if(del) EraseVector(X, N);
}

Vector::~Vector()
{
	FreeDblArray1D(X);
	n = 0;
}


/* ******************************************************* */
void Vector::resize(const int N, const bool del )
{
	if( N != n ) {
		n = N;
		FreeDblArray1D( X );
		X = NewDblArray1D( n );
		if( del ) EraseVector(X, n);
	}
}


double Vector::operator*(const Vector & y) const {
	if(n==y.n)
		return VecVecMul(X,y.X,n);
	else return -1.;
}

Vector& Vector::operator= (const Vector & y) {
	if( &y == this ) return (*this);
	if(n != y.n) {
		FreeDblArray1D( X );
		n	= y.n;
		X	= NewDblArray1D( n );
	}
	memcpy(X, y.X, sizeof(double)*n );
	return(*this);
}

Vector& Vector::operator= (const double a)
{
	if(a==0.)	memset(X,0,sizeof(double)*n);
	else		for(int i=0;i<n;X[i++]=a);;
	return(*this);
}

Vector& Vector::operator+= (const Vector & y)
{
	if( n != y.n ) {
		cerr << "ERROR: AddVectors - dimension mismatch" << endl;
		exit(1);
	}
	for(int i=0;i<n;i++) X[i]+=y.X[i];
	return(*this);
}

Vector& Vector::operator-= (const Vector & y)
{
	if( n != y.n ) {
		cerr << "ERROR: AddVectors - dimension mismatch" << endl;
		exit(1);
	}
	for(int i=0;i<n;i++) X[i]-=y.X[i];
	return(*this);
}

Vector& Vector::operator*= (const double a)
{
	for(int i=0;i<n;i++) X[i] *= a;
	return(*this);
}


Vector Vector::operator+(const Vector & y) const {
	Vector z = AddVectors(*this, y);
	return z;
}

Vector Vector::operator-(const Vector & y) const {
	Vector z = AddVectors(*this, y, -1.);
	return z;
}

#ifdef VECMAT_MPI
void Vector::MPIBroadcast(const int root)
{
	int	nproc, proc_id;
	MPI_Comm_rank( MPI_COMM_WORLD, &proc_id );
	MPI_Comm_size( MPI_COMM_WORLD, &nproc );
	if(nproc > 1) {
		const int n_old = n;
		MPI_Bcast(&n, 1, MPI_INT, root, MPI_COMM_WORLD );
		
		/* make sure the vector has the proper memory allocated to it */
		if( proc_id != root )
			if( n != n_old ) {
				delete [] X;
				X = new double [ n ];
			}
		MPI_Bcast(X, n, MPI_DOUBLE, root, MPI_COMM_WORLD ); /* distribute the data from the root to the children */
	}
}
#endif

Vector operator*(const Matrix & A, const Vector & y) {
	Vector z(A.m);
	cblas_dgemv(CblasRowMajor, CblasNoTrans, A.m, A.n, 1., A.GetArray1D(), A.n, y.X, 1, 0., z.X, 1);
	return z;
}

Vector operator*(const Vector & y, const Matrix & A) {
	Vector z(A.n);
	cblas_dgemv(CblasRowMajor, CblasTrans, A.m, A.n, 1., A.GetArray1D(), A.n, y.X, 1, 0., z.X, 1);
	return z;
}


/* ******************************************************* */
// friend
Vector operator-(const Vector & x)
{
	Vector y=x;
	y *= -1.;
	return y;
}
Vector operator/(const Matrix & A, const Vector & y)
{
	Vector x(y.n);
	SolveLinearSystem(A,y,x);
	return x;
}

Vector		NewVector( const int n ) {
	Vector x;
	x.n = n;
	x.X = NewDblArray1D( n );
	return x;
}

void		FreeVector( Vector & x ) {
	x.~Vector();
}

void		FreeMatrix( Matrix & M ) {
	M.~Matrix();
}

void		Vector::print() const
{
	for(int i=0;i<n;i++)printf("  %16.8lf",X[i]);
	printf("\n");
}

void		Matrix::print() const
{
	int k=0;
	for(int i=0;i<m;i++)
	{
		for(int j=0;j<n;j++)printf("  %16.8lf",A[0][k++]);
		printf("\n");
	}
}


void		ScalVecMul(		const double a, Vector & b ) {
	for(int i=0;i<b.n;b.X[i++]*=a);;
}

void		AddVectors(		Vector & x, const Vector & y, const Vector & z, const double a ) {
	if( y.n != z.n ) {
		cerr << "ERROR: AddVectors - dimension mismatch" << endl;
		exit (1);
	}
	if( x.n != y.n ) {
		FreeVector( x );
		x = NewVector( y.n);
	}
	AddVectors(x.X, y.X, z.X, x.n, a);
// 	for(int i=0;i<x.n;i++) x.X[i]=y.X[i]+a*z.X[i];
}

Vector		AddVectors(		const Vector & y, const Vector & z, const double a ) {
	Vector x = NewVector(y.n);
// 	AddVectors(x,y,z, a);
	AddVectors(x.X, y.X, z.X, x.n, a);
	return x;
}

void		EraseVector( Vector & x, const double z ) {
	EraseVector(x.X, x.n, z);
// 	for(int i=0;i<x.n;x.X[i++] = z);;
}


double		VectorNorm(const Vector & x, const int p ) {
	return VectorNorm(x.X, x.n, p);
}

double		VecVecMul( const Vector & x, const Vector & y )
{
	if(x.n != y.n) {
		cerr << "ERROR: VecVecMul - dimension mismatch" << endl;
		exit (1);
	}
	return VecVecMul( x.X, y.X, x.n );
}

void	MatVecMul( Vector & y, const Matrix & A, const Vector & x )
{
	if(A.n != x.n) {
		cerr << "ERROR: MatVecMul - dimension mismatch" << endl;
		exit (1);
	}
	if( y.n != A.m ) {
		FreeVector( y );
		y = NewVector( A.m );
	}
// 	MatVecMul( A.A, x.X, y.X, A.m, A.n );
	MatVecMul( A.GetArray2D(), x.X, y.X, A.m, A.n );
}

Vector	MatVecMul(		const Matrix & A, const Vector & x )
{
	Vector y = NewVector( A.m );
	MatVecMul( y, A, x );
	return y;
}


Vector	MatTVecMul(		const Matrix & A, const Vector & x )
{
	Vector y = NewVector( A.n );
// 	MatTVecMul(A.A, x.X, y.X, A.m, A.n );
	MatTVecMul(A.GetArray2D(), x.X, y.X, A.m, A.n );

	return y;
}


void	MatTVecMul(		Vector & y, const Matrix & A, const Vector & x )
{
	if(A.m != x.n) {
		cerr << "ERROR: MatTVecMul - dimension mismatch" << endl;
		exit (1);
	}
	if( y.n != A.n ) {
		FreeVector( y );
		y = NewVector( A.n );
	}
// 	MatTVecMul(A.A, x.X, y.X, A.m, A.n );
	MatTVecMul(A.GetArray2D(), x.X, y.X, A.m, A.n );
}







Matrix::Matrix()
{
	A = 0;
	m = n = 0;
	is_diag		= false;
	is_dummy	= false;
}

Matrix::Matrix(const int N, const bool del )
{
	A = 0;
	m = n = 0;
	resize(N,N,del);
	is_diag		= false;
	is_dummy	= false;
}

Matrix::Matrix(const int M, const int N, const bool del )
{
	A = 0;
	m = n = 0;
	resize(M,N,del);
	is_diag		= false;
	is_dummy	= false;
}

Matrix::Matrix(const Matrix & B)
{
	A=0;
	m = n = 0;
	resize(B.m,B.n,false);
	if((m != 0) && (n != 0)) memcpy(A[0], B.A[0], sizeof(double)*m*n);
	is_diag		= B.IsDiagonal();
	is_dummy	= false;
}

Matrix::Matrix(double * x, const int M, const int N )
{
	A[0] = x;
	is_dummy	= true;
	is_diag		= false;
	m = M;
	n = ((N>0)?N:m);
}

Matrix::~Matrix()
{
	if( A != 0 )
		if( !is_dummy )
			delete [] A[0];
	for(int i=0;i<m;i++) A[i] = 0;
	delete [] A;
	A	= 0;
	m	= n = 0;
	is_diag		= false;
	is_dummy	= false;
}

void Matrix::Identity() {
	is_diag = true;
	memset(A[0], 0, sizeof(double)*m*n);
	if( m <= n )	for(int i=0;i<m;i++) A[0][i*(n+1)] = 1.;
	else			for(int i=0;i<n;i++) A[0][i*(n+1)] = 1.;
}

#ifdef VECMAT_MPI
void Matrix::MPIBroadcast(const int root)
{
	int	nproc, proc_id;
	MPI_Comm_rank( MPI_COMM_WORLD, &proc_id );
	MPI_Comm_size( MPI_COMM_WORLD, &nproc );
	if(nproc > 1) {
		int		mnew = 0, nnew = 0;
		bool	dummy_new = is_dummy;
		if( proc_id == root ) { mnew = m; nnew = n; }
		MPI_Bcast(&mnew, 1, MPI_INT, root, MPI_COMM_WORLD );
		MPI_Bcast(&nnew, 1, MPI_INT, root, MPI_COMM_WORLD );
		MPI_Bcast(&dummy_new, 1, MPI_UNSIGNED_CHAR, root, MPI_COMM_WORLD );
		if( proc_id != root ) {
			if( ( is_dummy ) && ( !dummy_new ) ) {
				(*this).~Matrix();
				resize(mnew, nnew);
			}
			else resize(mnew, nnew,false);
			is_dummy = dummy_new;
		}
		MPI_Bcast(&is_diag, 1, MPI_UNSIGNED_CHAR, root, MPI_COMM_WORLD );
		MPI_Bcast(A[0], m*n, MPI_DOUBLE, root, MPI_COMM_WORLD );
	}
}

void Matrix::MPISendToFrom( const int to, const int from )
{
	int	nproc, proc_id;
	MPI_Comm_rank( MPI_COMM_WORLD, &proc_id );
	MPI_Comm_size( MPI_COMM_WORLD, &nproc );
	if( nproc > 1 )
	{
		if( proc_id == from ) {
			MPI_Send( &m,		1,	MPI_INT,			to, MPI_TAG_MATRIX, MPI_COMM_WORLD );
			MPI_Send( &n,		1,	MPI_INT,			to, MPI_TAG_MATRIX, MPI_COMM_WORLD );
			MPI_Send( &is_dummy,1,	MPI_UNSIGNED_CHAR,	to, MPI_TAG_MATRIX, MPI_COMM_WORLD );
			MPI_Send( &is_diag,	1,	MPI_UNSIGNED_CHAR,	to, MPI_TAG_MATRIX, MPI_COMM_WORLD );
			MPI_Send( A[0],		m*n,MPI_DOUBLE,			to, MPI_TAG_MATRIX, MPI_COMM_WORLD );
		}
		if( proc_id == to ) {
			MPI_Status mpi_st;
			int		mnew = 0, nnew = 0;
			bool 	dummy_new = false;
			MPI_Recv( &mnew,	1,	MPI_INT,				from, MPI_TAG_MATRIX, MPI_COMM_WORLD, &mpi_st );
			MPI_Recv( &nnew,	1,	MPI_INT,				from, MPI_TAG_MATRIX, MPI_COMM_WORLD, &mpi_st );
			MPI_Recv( &dummy_new,1,	MPI_UNSIGNED_CHAR,	from, MPI_TAG_MATRIX, MPI_COMM_WORLD, &mpi_st );
			if( ( is_dummy ) && ( !dummy_new ) ) {
				(*this).~Matrix();
				resize(mnew, nnew);
			}
			else resize(mnew, nnew,false);
			is_dummy = dummy_new;
			MPI_Recv( &is_diag,	1,	MPI_UNSIGNED_CHAR,	from, MPI_TAG_MATRIX, MPI_COMM_WORLD, &mpi_st );
			MPI_Recv( A[0],		m*n,MPI_DOUBLE,			from, MPI_TAG_MATRIX, MPI_COMM_WORLD, &mpi_st );
		}
	}
}
#endif

void Matrix::resize(const int newM, const int newN, const bool del )
{
	if( (newM==0) || (newN==0) ) { n=m=0; return; }
	if( (newM!=m) || (newN!=n) )
	{
		double * x;
		if(newM*newN != m*n)
		{
			if(A != 0)
			{
				if(!is_dummy) {
					delete [] A[0];
					for(int i=0;i<m;i++) A[i] = 0;
					x = new double [newM*newN];
				}
				else	x = A[0];
				delete [] A; A = 0;
			}
			else {
				x = new double [newM*newN]; /* if A == 0 */
			}
		}
		else { x = A[0]; delete [] A; A=0; }
		m = newM; n = newN;
		A = new double * [m];
		for(int i=0;i<m;i++) A[i]=x+n*i;
		x = 0;
	}
	else if( (m == 0) && (n == 0) && (A == 0) )
	{
		double *x = new double [newM*newN];
		m = newM; n = newN;
		A = new double * [m];
		for(int i=0;i<m;i++) A[i]=x+n*i;
		x = 0;
	}
	if(del) EraseVector(A[0],m*n);
	is_diag = false;
}


Matrix& Matrix::operator=(const Matrix & B) {
	if( &B == this ) return (*this);
	if( m*n != B.m  * B.n )
		resize(B.m,B.n,false);
	else {
		double * x = A[0];
		m=B.m; n=B.n;
		delete [] A;
		A = new double * [m];
		for(int i=0;i<m;i++) A[i]=x+i*n;
		x = 0;
	} /* keep old size, but change values */
	memcpy(A[0], B.A[0], sizeof(double)*m*n);
	SetDiagonal( B.IsDiagonal() );
	return (*this);
}

Matrix& Matrix::operator=(const double a) {
	EraseVector(A[0],m*n,a);
	is_diag = false;
	return(*this);
}

Matrix& Matrix::operator+=(const Matrix & B) {
	AddVectors( A[0], A[0], B.GetArray1D(), m*n );
	is_diag = ( IsDiagonal() && B.IsDiagonal() );
	return(*this);
}

Matrix& Matrix::operator-=(const Matrix & B) {
	AddVectors( A[0], A[0], B.GetArray1D(), m*n, -1. );
	is_diag = ( IsDiagonal() && B.IsDiagonal() );
	return(*this);
}

Matrix& Matrix::operator*=(const double a) {
	ScalVecMul( a, A[0], m*n );
	return(*this);
}

Matrix& Matrix::operator/=(const double a) {
	ScalVecMul( 1./a, A[0], m*n );
	return(*this);
}


Matrix Matrix::operator+(const Matrix & B) const {
	Matrix C = *this;
	C += B;
	return C;
}

Matrix Matrix::operator-(const Matrix & B) const {
	Matrix C = *this;
	C -= B;
	return C;
}

Matrix operator*(const Matrix & A, const double a) {
	Matrix C = A;
	C *= a;
	return C;
}

Matrix operator/(const Matrix & A, const double a) {
	Matrix C = A;
	C /= 1./a;
	return C;
}

Matrix operator*(const double a, const Matrix & A) {
	return A*a;
}

Vector operator*(const Vector & y, const double a)
{
	Vector z(y);
	for(int i=0; i<y.n;i++) z.X[i]*=a;
	return z;
}
Vector operator*(const double a,   const Vector & y)
{
	Vector z(y);
	for(int i=0; i<y.n;i++) z.X[i]*=a;
	return z;
}
Vector operator/(const Vector & y, const double a) 
{
	Vector z(y);
	for(int i=0; i<y.n;i++) z.X[i]/=a;
	return z;
}




void		Matrix::SymEigenValVec( Matrix & EV, Vector & lambda )
{
	/*
	 *	Return EV, such that (THIS)*EV = EV*diag(lambda)
	*/
	if(m!=n) {
		cerr << "ERROR in Matrix::EigenValVec: Matrix is not square!" << endl;
		exit(100);
	}
	EV.resize(m,m,true);
	lambda.resize(m,true);

// mkl_lapack.h:void dsyev( char* jobz, char* uplo, MKL_INT* n, double* a, MKL_INT* lda, double* w, double* work, MKL_INT* lwork, MKL_INT* info );
	char	jobz	= 'V', /* compute eigenvectors */
			uplo	= 'U'; /* only store upper part */
	double	*a		= NewDblArray1D(  m*m  ),
			*work	= NewDblArray1D(  4*m*m );	/* real part of eigenvalues */
	int		lwork	= 4*m*m,
			info	= 0;

	/* Fortran style ordering!!! */
	for(int i=0;i<m;i++)
		for(int j=i;j<m;j++)
			a[i+j*m]=A[i][j];
	
//	dsyev( &jobz, &uplo, &m, a, &m, lambda.X, work, &lwork, &info );
	
	if(info != 0) {
		cerr << "ERROR in SymEigenValVec: info = " << info << " (from LAPACK dsyev)" << endl;
		exit(199);
	}
	for(int i=0;i<m;i++)
		for(int j=0;j<m;j++)
			EV.A[i][j] = a[i+j*m];

	FreeDblArray1D( work );
	FreeDblArray1D( a );
}


void Matrix::transpose()
{
	int i=0,j=0;
	if(m==n)
	{
		double x=0.;
		for(i=0;i<n;i++)
			for(j=i+1;j<n;j++)
			{
				x=A[i][j];
				A[i][j]=A[j][i];
				A[j][i]=x;
			}
	}
	else
	{
		int		k	= 0;
		double	* y	= new double [m*n];
		/* temporary copy */
		memcpy( y, A[0], sizeof(double)*m*n );
		/* transpose entries: AT(j,i) = A(i,j) */
		for(i=0;i<m;i++)
			for(j=0;j<n;j++)
				A[0][j*m+i] = y[k++];
		/* change second order pointer accordingly */
		delete [] A;
		A = new double * [n];
		for(i=1; i<n; i++) A[i] = A[0]+i*m;

		/* swap m and n */
		i = m;	m = n;	n = i;
		/* free temporary memory */
		delete [] y; y = 0;
	}
}


/* *********************************************************************** */
void Vector::ReadDataFromFile( FILE * F, const int N )
{
	resize(N);
	for(int i=0; i<n; i++) fscanf(F,"%lf",X+i);
}
/* *********************************************************************** */
void Matrix::ReadDataFromFile( FILE * F, const int M, const int N )
{
	resize(M, N);
	for(int i=0; i<m*n; i++) fscanf(F,"%lf",A[0]+i);
}
/* *********************************************************************** */
Matrix		NewMatrix( const int m, const int n ) {
	Matrix M( m,( n == 0 ) ? m : n );
	return M;
}

Matrix		MatCopy(	const Matrix & B ) {
	Matrix A=B;
	return A;
}

void		MatCopy(	Matrix & A, const Matrix & B )
{
	A = B;
}

Matrix		MIdentityMatrix(const int n ) {
	Matrix A = NewMatrix(n,n);
	A.Identity();
	return A;
}

void		GetTransposedMat(	Matrix & A ) {
	if(A.n != A.m) {
		Matrix Atmp(A);
		A.resize(A.n,A.m,false);
		for(int i=0;i<A.m;i++)
			for(int j=0;j<A.n;j++)
				A(i,j) = Atmp(j,i);
	}
	else {
		double x=0.;
		for(int i=0;i<A.m;i++)
			for(int j=i;j<A.n;j++)
			{
				x		= A(i,j);
				A(i,j)	= A(j,i);
				A(j,i)	= x;
			}
	}
}

Matrix		TransposeMat(		const Matrix & A ) {
	Matrix AT(A.n, A.m);
	for(int i=0;i<A.m;i++)
		for(int j=0;j<A.n;j++)
			AT(j,i) = A(i,j);
	return AT;
}
void		EraseMatrix( Matrix & M, const double z ) {
	M=z;
}

void	MatAddMul(		Matrix & A, const Matrix & B, const double z ) {
	if((A.m!=B.m) || ( A.n!=B.n)) {
		cerr << "ERROR: MatAddMul - dimension mismatch" << endl;
		exit (1);
	}
	AddVectors( A.GetArray1D(), A.GetArray1D(), B.GetArray1D(), A.m*A.n, z);
}

void	MatMultiply(	Matrix & A, const double z ) {
	A *= z;
}

void 	MatMatMul(			const Matrix & A, const Matrix & B,	Matrix & C )	{	BLAS_MatMatMul(A,B,C);	}
void		MatTMatMul(		const Matrix & A, const Matrix & B, Matrix & C )	{	BLAS_MatTMatMul(A,B,C);	}
void		MatMatTMul(		const Matrix & A, const Matrix & B, Matrix & C )	{	BLAS_MatMatTMul(A,B,C);}
void		MatTMatTMul(	const Matrix & A, const Matrix & B, Matrix & C )	{	BLAS_MatTMatTMul(A,B,C);	}

Matrix		MatMatMul(		const Matrix & A, const Matrix & B )
{	Matrix C(A.m,B.n);
	BLAS_MatMatMul(A,B,C);
	return C;
}
Matrix		MatTMatMul(		const Matrix & A, const Matrix & B )
{
	Matrix C(A.n,B.n);
	BLAS_MatTMatMul(A,B,C);
	return C;
}
Matrix		MatMatTMul(		const Matrix & A, const Matrix & B )
{
	Matrix C(A.m,B.m);
	BLAS_MatMatTMul(A,B,C);
	return C;
}
Matrix		MatTMatTMul(		const Matrix & A, const Matrix & B ) {
	Matrix C(A.n,B.m);
	BLAS_MatTMatTMul(A, B, C);
	return C;
}

Matrix operator*(const Matrix & A, const Matrix & B)
{
	return MatMatMul(A, B);
}

void		SolveLinearSystems( const Matrix & A, const Matrix & Y, Matrix & X) {
	SolveLinearSystems(A.GetArray2D(), Y.GetArray2D(), X.GetArray2D(), A.m, Y.n);
}
void		SolveLinearSystem( const Matrix & A, const Vector & y, Vector & x) {
	SolveLinearSystem(A.GetArray2D(), y.X, x.X, A.m);
}
void		InvertMatrix(	const Matrix & A, Matrix & invA )
{
	invA.resize(A.m, A.m, false);
	double ** invA_A = invA.GetArray2D();
	InvertMatrix(A.GetArray2D(),invA_A,A.m);
	invA_A = 0;
}

Matrix		InvertMatrix(	const Matrix & A )
{
	Matrix invA = NewMatrix(A.m, A.m);
	InvertMatrix(A, invA);
	return invA;
}

double		Det3x3(			Matrix & A )
{
	return Det3x3(A.GetArray2D());
}

// /* ****************************************************************** */
// void BLAS_MatMatMul(	const Matrix & A, const Matrix & B, Matrix & C )
// {
// 	C.resize(A.m, B.n, false);
// 	if( A.IsDiagonal() && B.IsDiagonal() ) {
// 		C.SetDiagonal( true );
// 		C=0.;
// 		for(int i=0;i<A.m;i++) C(i,i)=A(i,i)*B(i,i);
// 	} else {
// 		if( A.IsDiagonal() ) {
// 			int k=0;
// 			double * CA = C.GetArray1D(), * BA = B.GetArray1D();
// 			for(int i=0;i<A.m;i++)
// 			{
// 				const double a = A(i,i);
// 				for(int j=0;j<B.n;j++)	CA[k] = a*BA[k++];
// 			}
// 			CA = 0; BA = 0;
// 		}
// 		else if( B.IsDiagonal() ) {
// 			int k=0;
// 			double * CA = C.GetArray1D(), * AA = A.GetArray1D();
// 			double b[B.m];
// 			for(int i=0;i<B.m;i++) b[i]=B(i,i);
// 			for(int i=0;i<A.m;i++)
// 			{
// 				for(int j=0;j<B.n;j++)	CA[k] = b[j]*AA[k++];
// 			}
// 			CA = 0; AA = 0;
// 		}
// 		else {
// 			cblas_dgemm(	CblasRowMajor,
// 							CblasNoTrans,
// 							CblasNoTrans,
// 							A.m, A.n, B.n, 1.0,
// 							A.GetArray1D(), A.n,
// 							B.GetArray1D(), B.n,
// 							0.0,
// 							C.GetArray1D(), C.n
// 						);
// 		}
// 	}
// }
// void BLAS_MatTMatMul(	const Matrix & A, const Matrix & B, Matrix & C )
// {
// 	C.resize(A.n, B.n, false);
// 	cblas_dgemm(	CblasRowMajor,
// 					CblasTrans,
// 					CblasNoTrans,
// 					A.m, A.n, B.n, 1.0,
// 					A.GetArray1D(), A.n,
// 					B.GetArray1D(), B.n,
// 					0.0,
// 					C.GetArray1D(), C.n
// 				);
// }
// void BLAS_MatMatTMul(	const Matrix & A, const Matrix & B, Matrix & C )
// {
// 	C.resize(A.m, B.m, false);
// // 	printf("MatMatTMul\n");
// // 	printf("A.m = %3d     -     A.n = %3d\n", A.m, A.n );
// // 	printf("B.m = %3d     -     B.n = %3d\n", B.m, B.n );
// // 	printf("C.m = %3d     -     C.n = %3d\n", C.m, C.n );
// 	cblas_dgemm(	CblasRowMajor,
// 					CblasNoTrans,
// 					CblasTrans,
// 					A.m, A.n, B.n, 1.0,
// 					A.GetArray1D(), A.n,
// 					B.GetArray1D(), B.n,
// 					0.0,
// 					C.GetArray1D(), C.n
// 				);
// }
// void BLAS_MatTMatTMul(	const Matrix & A, const Matrix & B, Matrix & C )
// {
// 	C.resize(A.n, B.n, false);
// 	cblas_dgemm(	CblasRowMajor,
// 					CblasTrans,
// 					CblasTrans,
// 					A.m, A.n, B.n, 1.0,
// 					A.GetArray1D(), A.n,
// 					B.GetArray1D(), B.n,
// 					0.0,
// 					C.GetArray1D(), C.n
// 				);
// }

/* ****************************************************************** */
void BLAS_MatMatMul(	const Matrix & A, const Matrix & B, Matrix & C )
{
	C.resize(A.m, B.n, false);
	if( A.IsDiagonal() && B.IsDiagonal() ) {
		C.SetDiagonal( true );
		C=0.;
		for(int i=0;i<A.m;i++) C(i,i)=A(i,i)*B(i,i);
	} else {
		if( A.IsDiagonal() ) {
			int k=0;
			double * CA = C.GetArray1D(), * BA = B.GetArray1D();
			for(int i=0;i<A.m;i++)
			{
				const double a = A(i,i);
				for(int j=0;j<B.n;j++)	CA[k] = a*BA[k++];
			}
			CA = 0; BA = 0;
		}
		else if( B.IsDiagonal() ) {
			int k=0;
			double * CA = C.GetArray1D(), * AA = A.GetArray1D();
			double b[B.m];
			for(int i=0;i<B.m;i++) b[i]=B(i,i);
			for(int i=0;i<A.m;i++)
			{
				for(int j=0;j<B.n;j++)	CA[k] = b[j]*AA[k++];
			}
			CA = 0; AA = 0;
		}
		else {
			cblas_dgemm(	CblasRowMajor,
							CblasNoTrans,
							CblasNoTrans,
							C.m, C.n, A.n, 1.0,
							A.GetArray1D(), A.n,
							B.GetArray1D(), B.n,
							0.0,
							C.GetArray1D(), C.n
						);
		}
	}
}
void BLAS_MatTMatMul(	const Matrix & A, const Matrix & B, Matrix & C )
{
	C.resize(A.n, B.n, false);
	cblas_dgemm(	CblasRowMajor,
					CblasTrans,
					CblasNoTrans,
					C.m, C.n, A.m, 1.0,
					A.GetArray1D(), A.n,
					B.GetArray1D(), B.n,
					0.0,
					C.GetArray1D(), C.n
				);
}
void BLAS_MatMatTMul(	const Matrix & A, const Matrix & B, Matrix & C )
{
	C.resize(A.m, B.m, false);
// 	printf("MatMatTMul\n");
// 	printf("A.m = %3d     -     A.n = %3d\n", A.m, A.n );
// 	printf("B.m = %3d     -     B.n = %3d\n", B.m, B.n );
// 	printf("C.m = %3d     -     C.n = %3d\n", C.m, C.n );
	cblas_dgemm(	CblasRowMajor,
					CblasNoTrans,
					CblasTrans,
					C.m, C.n, A.n, 1.0,
					A.GetArray1D(), A.n,
					B.GetArray1D(), B.n,
					0.0,
					C.GetArray1D(), C.n
				);
}
void BLAS_MatTMatTMul(	const Matrix & A, const Matrix & B, Matrix & C )
{
	C.resize(A.n, B.n, false);
	cblas_dgemm(	CblasRowMajor,
					CblasTrans,
					CblasTrans,
					C.m, C.n, A.m, 1.0,
					A.GetArray1D(), A.n,
					B.GetArray1D(), B.n,
					0.0,
					C.GetArray1D(), C.n
				);
}
/* ****************************************************************** */
void		PrintFullMatrix(double ** A, const int n, const int m)
{
	for(int i=0;i<n;i++)
	{
		for(int j=0;j<( (m<0)?n:m); j++) printf("  %14.8lE", A[i][j]);
		printf("\n");
	}
}

/* ****************************************************************** */
double**	IdentityMatrix(	const int n )
{
	double ** A = NewDblArray2D(n,n);
	int i;
	for(i=0;i<n;i++) A[0][i] = 0.;
	for(i=1;i<n;i++) memcpy(A[i],A[0], sizeof(double)*n);
	for(i=0;i<n;i++) A[i][i] = 1.;
	return A;
}

double**	MatCopy(	double ** A, const int n, const int m )
{
	double ** B = NewDblArray2D( n, m );
	for(int i=0;i<n;i++) memcpy(B[i],A[i],sizeof(double)*m);
	return B;
}

/* ******************************************************* */

void		GetTransposedMat(	double **&A, const int n, const int m )
{
	int i,j;
	if( n != m)
	{
		double ** tmpA = NewDblArray2D(m,n);
		for(i=0;i<n;i++)
			for(j=0;j<m;j++)
				tmpA[j][i] = A[i][j];
		FreeDblArray2D(A, m);
		A = tmpA;
	}
	else {
		double x = 0.;
		for(i=0;i<n;i++)
			for(j=i+1;j<n;j++)
			{ x = A[i][j]; A[i][j] = A[j][i]; A[j][i] = x; }
	}
}

double**	TransposeMat(	double ** A, const int n, const int m )
{
	int i,j;
	double ** tmpA = NewDblArray2D(m,n);

	for(i=0;i<n;i++)
		for(j=0;j<m;j++)
			tmpA[j][i] = A[i][j];
	return tmpA;
}

/* ******************************************************* */

double VectorNorm( const double * x, const int n, const int p )
{
	double norm = 0.;
	if( p==1 ) {
		for(int i=0;i<n;i++) norm+=fabs(x[i]);
	}
	else if ( p==2 ) {
		for(int i=0;i<n;i++) norm+=x[i]*x[i];
		norm = sqrt(norm);
	}
	else if ( p==-2) {
// 		int maxidx = 0;
		for(int i=0;i<n;i++)
			if(norm<fabs(x[i])) {
				norm= fabs(x[i]);
// 				maxidx = i; 
			}
	}
	else if ( p==-1) {
		for(int i=0;i<n;i++)
			norm = ((norm < fabs(x[i]))?fabs(x[i]):norm);
	}
	else {
		for(int i=0;i<n; i++) norm += pow(x[i], p);
		norm = pow(norm, 1./p);
	}
	return norm;
}

double VectorDistance(const double * x, const double * y, const int n)
{
	double v = 0.;
	for(int i=0;i<n;i++) v+=(x[i]-y[i])*(x[i]-y[i]);
	return sqrt(v);
}

double DeviNorm(const double * x )
{
	const double p = (x[0]+x[1]+x[2])/3.;
	const double q = x[0]*x[0]+x[1]*x[1]+x[2]*x[2]+x[3]*x[3]+x[4]*x[4]+x[5]*x[5];
	const double r = q - 3.*p*p;
	return (( r < 0. ) ? 0.:sqrt(r)); /* r may become less than zero due to round off!!! */
	
}

void ScalVecMul( const double a, double * b, const int n)
{	for(int i=0;i<n;i++) *b++ *= a; }

void AddVectors( double * z, const double * x, const double * y, const int n, const double a)
{	if(a == 1.)			for(int i=0;i<n;i++) z[i] = x[i] + y[i];
	else if(a == -1.)	for(int i=0;i<n;i++) z[i] = x[i] - y[i];
	else				for(int i=0;i<n;i++) z[i] = x[i] + a*y[i];
}

double * AddVectors( const double * x, const double * y, const int n, const double a )
{
	double *z = new double [n];
	AddVectors(z,x,y,n,a);
	return z;
}

double * VecLinComb( double ** X, double * a, const int n, const int m )
{
	double *z = new double [n];
	int i,j;
	for(j=0;j<n;j++) {
		z[j] = 0.;
		for(i=0;i<m;i++)	z[j] += a[i]*X[j][i];
	}
	return z;
}

double VecVecMul( const double * x, const double * y, const int n ) {
	double v=0.;
	int i=0;
	for(i=0;i<n;i++) v+=x[i]*y[i];
	return v;
}

double * MatVecMul(
		double ** A, const double * x,
		const int n, const int m)
{
	double * y = new double[n];
	MatVecMul(A, x, y, n, m);
	return y;
}

void MatVecMul(
		double ** A, const double * x, double * y,
		const int n, const int m)
{
	int i,j;
	for(i=0;i<n;i++) {
		y[i] = 0.;
		for(j=0;j<m;j++) y[i] += A[i][j]*x[j];
	}
}

void MatTVecMul(
		double ** A, const double * x, double * y,
		const int n, const int m)
{
	int i,j;
	for(i=0;i<m;i++) {
		y[i] = 0.;
		for(j=0;j<n;j++) y[i] += A[j][i]*x[j];
	}
}

double * MatTVecMul(
		double ** A, const double * x,
		const int n, const int m)
{
	double * y = new double[m];
	MatTVecMul(A,x,y,n,m);
	return y;
}

/* ****************************************************************** */

void		EraseVector( double * x, const int N, const double z )
{
	int i;
	if( z == 0 )	memset( x, 0, sizeof(double)*N );
	else for(i=0;i<N;i++) x[i]=z;
}

void		EraseMatrix( double ** x, const int N, const int M, const double z )
{
	int i,j;
	EraseVector( x[0], M, z );	/* ERASE FIREST ROW, COPY TO ALL OTHER ENTRIES */
	for(i=1;i<N;i++) memcpy(x[i], x[0], sizeof(double)*M);
}

void		MatAddMul( double **& A, double ** B,	const int n, const int m, const double z )
{
	int i,j;
	if( z != 1. )	for(i=0;i<n;i++) for(j=0;j<m;j++) A[i][j] += z*B[i][j];
	else			for(i=0;i<n;i++) for(j=0;j<m;j++) A[i][j] += B[i][j];
}

void		MatMultiply(double ** &A, const int n, const int m, const double z )
{
	int i,j;
	if(z!=1.) for(i=0;i<n;i++) for(j=0;j<m;j++) A[i][j] *= z;
}

/* ****************************************************************** */

double ** MatMatMul(
		double ** A, double ** B,
		const int n, const int m, const int o, const int p)
{
	double ** C = NewDblArray2D( n, p );
	MatMatMul( A, B, C, n, m, o, p );
	return C;
}

/* ****************************************************************** */
void MatMatMul(
		double ** A, double ** B, double ** C,
		const int n, const int m, const int o, const int p)
{
	if( m != o ) {
		fprintf(stderr,"ERROR: Trying to get A * B, with dim(A) = (%d,%d) and dim(B) = (%d,%d)\n", n,m,o,p);
		exit(99);
	}
	int i,j,k;
	for(i=0;i<n;i++)
		for(j=0;j<p;j++) {
			C[i][j]=0.;
			for(k=0;k<m;k++)
				C[i][j] += A[i][k]*B[k][j];
		}
}

/* ****************************************************************** */

void	MatTMatMul(		double ** A, double ** B, double ** C,
							const int n, const int m, const int o, const int p)
{
	if( n != o ) {
		fprintf(stderr,"ERROR: Trying to get A^T * B, with dim(A) = (%d,%d) and dim(B) = (%d,%d)\n", n,m,o,p);
		exit(99);
	}
	int i,j,k;
	for(i=0;i<m;i++)
		for(j=0;j<p;j++) {
			C[i][j]=0.;
			for(k=0;k<n;k++)
				C[i][j] += A[k][i]*B[k][j];
		}
}
/* ****************************************************************** */

double**	MatTMatMul(		double ** A, double ** B,
							const int n, const int m, const int o, const int p)
{
	double ** C = NewDblArray2D( m, p );
	MatTMatMul(A,B,C,n,m,o,p);
	return C;
}

/* ****************************************************************** */

void	MatMatTMul(		double ** A, double ** B, double ** C, const int n, const int m, const int o, const int p)
{
	if( m != p ) {
		fprintf(stderr,"ERROR: Trying to get A * B^T, with dim(A) = (%d,%d) and dim(B) = (%d,%d)\n", n,m,o,p);
		exit(99);
	}
	
	int i,j,k;
	for(i=0;i<n;i++)
		for(j=0;j<o;j++) {
			C[i][j]=0.;
			for(k=0;k<m;k++)
				C[i][j] += A[i][k]*B[j][k];
		}
}

/* ****************************************************************** */
double**	MatMatTMul(		double ** A, double ** B, const int n, const int m, const int o, const int p)
{
	double ** C =	NewDblArray2D( n, o );
	MatMatTMul(A,B,C,n,m,o,p);
	return C;
}

/* ****************************************************************** */

void	MatTMatTMul(		double ** A, double ** B, double ** C, const int n, const int m, const int o, const int p)
{
	if( n != p ) {
		fprintf(stderr,"ERROR: Trying to get A^T * B^T, with dim(A) = (%d,%d) and dim(B) = (%d,%d)\n", n,m,o,p);
		exit(99);
	}
	int i,j,k;
	for(i=0;i<m;i++)
		for(j=0;j<o;j++) {
			C[i][j]=0.;
			for(k=0;k<n;k++)
				C[i][j] += A[k][i]*B[j][k];
		}
}

/* ****************************************************************** */

double**	MatTMatTMul(		double ** A, double ** B,	const int n, const int m, const int o, const int p)
{
	double ** C =	NewDblArray2D( m, o );
	MatTMatTMul(A,B,C,n,m,o,p);
	return C;
}

/* ****************************************************************** */

double Det3x3( double ** A ) 
{
	return			(A[0][0]*(A[1][1]*A[2][2]-A[2][1]*A[1][2])
				+	A[1][0]*(A[2][1]*A[0][2]-A[0][1]*A[2][2])
				+	A[2][0]*(A[0][1]*A[1][2]-A[1][1]*A[0][2]));
}

/* ********************************************************************** */

void SolveLinearSystem(double ** A, const double * y, double * x, const int N)
{
	double ** xx = NewDblArray2D(N,1), ** yy = NewDblArray2D(N,1);
	int i;
	for(i=0;i<N;i++) yy[i][0] = y[i];
	SolveLinearSystems(A,yy,xx,N,1);
	for(i=0;i<N;i++) x[i]= xx[i][0];
	FreeDblArray2D( xx, N );
	FreeDblArray2D( yy, N );
}

void SolveLinearSystems(double ** A, double ** y, double ** x, const int N, const int M)
{
	int		idx=0;
	double	tmp = 0.;
	double	B[N][N+M];
	
	for(int i=0;i<N;i++) for(int j=0;j<N;j++) B[i][j]	= A[i][j];
	for(int i=0;i<N;i++) for(int j=0;j<M;j++) B[i][N+j]	= y[i][j];
	
	for(int i=0;i<N;i++) 
	{
		idx = -1;		tmp = 0.; /* hier eigentlich: 1.e-10 * || A || */
		for(int j=i;j<N;j++)
			if( fabs(B[j][i]) > 2.*tmp ) { tmp = fabs(B[j][i]); idx=j; }
		if(idx < 0) {
			std::cerr << "ERROR: SolveLinearSystem: No pivot element found (i=" << i << ", idx=" << idx << ",  N=" << N << ")" << std::endl;
			for(int j=0;j<N;j++) {
				for(int k=0;k<N;k++) printf("  %12.6lf",A[i][j]);
				printf("\n");
			}
			printf("--------------------------\n");
// 			exit(99);
		}
		if(idx != i) {
			/* Switch Pivot Elements */
			for(int j=i;j<N+M;j++) {
				tmp			= B[idx][j];
				B[idx][j]	= B[i][j];
				B[i][j]		= tmp;
			}
		}
		for(int j=i+1; j<N;j++)
		{
			/* Save some computational time */
			if(B[j][i] != 0.)
			{
				tmp = B[j][i] / B[i][i];
				for(int k=i;k<N+M;k++) B[j][k] -= B[i][k]*tmp;
			}
		}
	}
	
	/* Compute the solution vector using backwards-substitution */
	for(int i=1;i<=N;i++) {
		for(int k=0;k<M;k++)
		{
			x[N-i][k] = B[N-i][N+k];
			for(int j=N+1-i;j<N;j++)
				x[N-i][k] -= B[N-i][j]*x[j][k];
			x[N-i][k] /= B[N-i][N-i];
		}
	}
}

/* ****************************************************************** */




/* ****************************************************************** */

void		InvertMatrix(	double ** A, double **&invA, const int N )
{ 
	double	** B = IdentityMatrix(N);
	SolveLinearSystems( A, B, invA, N, N );
	FreeDblArray2D(B,N);
}

double**	InvertMatrix(	double ** A, const int N )
{
	double ** invA = NewDblArray2D(N,N);
	InvertMatrix(A, invA, N);
	return invA;
}
/* ****************************************************** */

double**	FullMatrix( const CSRMat & K)
{
	int i,j;
	double ** Ktot = NewDblArray2D(K.dim, K.dim);
	EraseMatrix(Ktot, K.dim, K.dim);
	for(i=0;i<K.dim;i++)
		for(j=K.ap[i];j<K.ap[i+1];j++)
			Ktot[i][K.ai[j]] += K.a[j];
	return Ktot;
}



#endif /* _FE_VECMAT_CXX_ */

